/*!
  @file triangulation.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include "mrover_vo/triangulation.h"
#include <opencv2/highgui/highgui.hpp>

#include <ros/ros.h>

using std::vector;
using cv::Mat;

namespace vo {

void Triangulation::compute() {
  if (!initCompute()) {
    ROS_ERROR("in Triangulation. perhaps you forgot to set features or camera parameters??");
    return;
  }

  // TODO need a comment here?
  // 
  cv::SVD svd;
  sp_->resize(feature_->matches->size());
  Mat A(4, 4, CV_64F);
  Mat X(4, 1, CV_64F);
  vector<cv::Point2d> m(2);
  for (size_t i = 0; i < feature_->matches->size(); ++i) {
    m[0].x = feature_->kp_vec[0]->at(feature_->matches->at(i).queryIdx).pt.x;
    m[0].y = feature_->kp_vec[0]->at(feature_->matches->at(i).queryIdx).pt.y;
    m[1].x = feature_->kp_vec[1]->at(feature_->matches->at(i).trainIdx).pt.x;
    m[1].y = feature_->kp_vec[1]->at(feature_->matches->at(i).trainIdx).pt.y;

    for (int d = 0; d < 2; ++d) {
      A.at<double>(2 * d, 0)     = P[d].at<float>(2, 0) * m[d].x - P[d].at<float>(0, 0);
      A.at<double>(2 * d, 1)     = P[d].at<float>(2, 1) * m[d].x - P[d].at<float>(0, 1);
      A.at<double>(2 * d, 2)     = P[d].at<float>(2, 2) * m[d].x - P[d].at<float>(0, 2);
      A.at<double>(2 * d, 3)     = P[d].at<float>(2, 3) * m[d].x - P[d].at<float>(0, 3);
      A.at<double>(2 * d + 1, 0) = P[d].at<float>(2, 0) * m[d].y - P[d].at<float>(1, 0);
      A.at<double>(2 * d + 1, 1) = P[d].at<float>(2, 1) * m[d].y - P[d].at<float>(1, 1);
      A.at<double>(2 * d + 1, 2) = P[d].at<float>(2, 2) * m[d].y - P[d].at<float>(1, 2);
      A.at<double>(2 * d + 1, 3) = P[d].at<float>(2, 3) * m[d].y - P[d].at<float>(1, 3);
    }

    svd(A);
    X = svd.vt(cv::Rect(0, 3, 4, 1)).t();
    //cv::SVD::solveZ(A, X);
    sp_->at(i).x = X.at<double>(0, 0) / X.at<double>(3, 0) * 10; 
    sp_->at(i).y = X.at<double>(1, 0) / X.at<double>(3, 0) * 10;
    sp_->at(i).z = X.at<double>(2, 0) / X.at<double>(3, 0) * 10;

#if 0
    X.at<double>(0, 0) = X.at<double>(0, 0) / X.at<double>(3, 0);
    X.at<double>(1, 0) = X.at<double>(1, 0) / X.at<double>(3, 0);
    X.at<double>(2, 0) = X.at<double>(2, 0) / X.at<double>(3, 0);
    X.at<double>(3, 0) = 1.0;
#endif
  }

}

}  // namespace vo
